/*=+--+=#=+--     Unmanned Aerial System Management Software      --+=#=+--+=#*\
|          Copyright (C) 2011 Regents of the University of Colorado.           |
|                             All Rights Reserved.                             |

     This program is free software: you can redistribute it and/or modify
     it under the terms of the GNU General Public License version 2 as
     published by the Free Software Foundation.

     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.

     You should have received a copy of the GNU General Public License
     along with this program.  If not, see <http://www.gnu.org/licenses/>.

            Jack Elston                       Cory Dixon                        
|           elstonj@colorado.edu              dixonc@colorado.edu              |
|                                                                              |
\*=+--+=#=+--                 --+=#=+--+=#=+--                    --+=#=+--+=#*/

/*!
  \file threadedPipe.cxx
  \brief 

  \author Jack Elston
  $Date: 2005/02/12 00:20:45 $
*/

#include <unistd.h>
#include <signal.h>
#include <stdlib.h>
#include <stdio.h>
#include <errno.h>
#include <fcntl.h>
#include <math.h>
#include <socket.h>
# include <arpa/inet.h>

#include "interface.h"

/*<-------Defines--------->*/
/*<------End Defines------>*/

/*<---Global Variables---->*/
/*! \brief determines level of output to user. */
extern bool verbose;
/*<-End Global Variables-->*/

Interface::Interface(String a_name) : name(a_name), my_systems() 
{
	my_subnet.s_addr = INADDR_ANY;
	my_port = 0;

	comm[0] = NULL;
	comm[1] = NULL;
	tran[0] = NULL;
	tran[1] = NULL;
	comp    = NULL;

	has_external_if = false;
	is_gateway      = false;
	can_command     = true;
}

/************************************************************\
  |                     Interface::addFifo                     |
  \************************************************************/
/*!
  \brief
  \param
  \pre
  \post
  \return
  */
void Interface::setCommBlock(ThreadedPipe * block)
{
	//cout << "Interface::setCommBlock" << endl;
	switch(block->mode()){
		case ThreadedPipe::TX:
			comm[1] = block;
			if((tran[0] != NULL) && (tran[0]->mode() == ThreadedPipe::DUPLEX)) 
				block->addFifo(ThreadedPipe::INBOARD_RX,tran[0]->getFifoName(ThreadedPipe::OUTBOARD_TX));
			else if(tran[1] != NULL)
				block->addFifo(ThreadedPipe::INBOARD_RX,tran[1]->getFifoName(ThreadedPipe::OUTBOARD_TX));
			else 
				block->addFifo(ThreadedPipe::INBOARD_RX);
			break;

		case ThreadedPipe::DUPLEX:
			if(comm[0] == NULL) {
				comm[1] = NULL;
				if((tran[0] != NULL) && (tran[0]->mode() == ThreadedPipe::DUPLEX)) 
					block->addFifo(ThreadedPipe::INBOARD_RX,tran[0]->getFifoName(ThreadedPipe::OUTBOARD_TX));
				else if(tran[1] != NULL)
					block->addFifo(ThreadedPipe::INBOARD_RX,tran[1]->getFifoName(ThreadedPipe::OUTBOARD_TX));
				else 
					block->addFifo(ThreadedPipe::INBOARD_RX);
			}
			else {
				comm[1] = block;
				block->addFifo(ThreadedPipe::INBOARD_RX); //FIXME
				block->addFifo(ThreadedPipe::INBOARD_TX); //FIXME
				break;
			}

		case ThreadedPipe::RX:
			comm[0] = block;
			if(tran[0] != NULL) 
				block->addFifo(ThreadedPipe::INBOARD_TX,tran[0]->getFifoName(ThreadedPipe::OUTBOARD_RX));
			else 
				block->addFifo(ThreadedPipe::INBOARD_TX);
			break;
	}
}

void Interface::setTranBlock(ThreadedPipe * block)
{
	//cout << "Interface::setTranBlock" << endl;
	switch(block->mode()){
		case ThreadedPipe::TX:
			tran[1] = block;

			if(comp != NULL)
				block->addFifo(ThreadedPipe::INBOARD_RX,comp->getFifoName(ThreadedPipe::OUTBOARD_TX));
			else 
				block->addFifo(ThreadedPipe::INBOARD_RX);

			if((comm[0] != NULL) && (comm[0]->mode() == ThreadedPipe::DUPLEX)) 
				block->addFifo(ThreadedPipe::OUTBOARD_TX,comm[0]->getFifoName(ThreadedPipe::INBOARD_RX));
			else if(comm[1] != NULL)
				block->addFifo(ThreadedPipe::OUTBOARD_TX,comm[1]->getFifoName(ThreadedPipe::INBOARD_RX));
			else 
				block->addFifo(ThreadedPipe::OUTBOARD_TX);

			break;
		case ThreadedPipe::DUPLEX:
			if(tran[0] == NULL) {
				tran[1] = NULL;

				if(comp != NULL)
					block->addFifo(ThreadedPipe::INBOARD_RX,comp->getFifoName(ThreadedPipe::OUTBOARD_TX));
				else 
					block->addFifo(ThreadedPipe::INBOARD_RX);

				if((comm[0] != NULL) && (comm[0]->mode() == ThreadedPipe::DUPLEX)) 
					block->addFifo(ThreadedPipe::OUTBOARD_TX,comm[0]->getFifoName(ThreadedPipe::INBOARD_RX));
				else if(comm[1] != NULL)
					block->addFifo(ThreadedPipe::OUTBOARD_TX,comm[1]->getFifoName(ThreadedPipe::INBOARD_RX));
				else 
					block->addFifo(ThreadedPipe::OUTBOARD_TX);
			}
			else {
				tran[1] = block;
				block->addFifo(ThreadedPipe::INBOARD_TX,tran[0]->getFifoName(ThreadedPipe::INBOARD_TX));
				block->addFifo(ThreadedPipe::OUTBOARD_RX,comm[1]->getFifoName(ThreadedPipe::INBOARD_TX));
				block->addFifo(ThreadedPipe::OUTBOARD_TX,comm[1]->getFifoName(ThreadedPipe::INBOARD_RX));
				break;
			}

		case ThreadedPipe::RX:
			tran[0] = block;
			if(comp != NULL)
				block->addFifo(ThreadedPipe::INBOARD_TX,comp->getFifoName(ThreadedPipe::OUTBOARD_RX));
			else 
				block->addFifo(ThreadedPipe::INBOARD_TX);

			if(comm[0] != NULL)
				block->addFifo(ThreadedPipe::OUTBOARD_RX,comm[0]->getFifoName(ThreadedPipe::INBOARD_TX));
			else 
				block->addFifo(ThreadedPipe::OUTBOARD_RX);

			break;
	}
}

void Interface::setCompBlock(ThreadedPipe * block)
{

	comp = block;

	switch(block->mode()){
		case ThreadedPipe::TX:
			block->addFifo(ThreadedPipe::INBOARD_RX);
			if((tran[0] != NULL) && (tran[0]->mode() == ThreadedPipe::DUPLEX)) 
				block->addFifo(ThreadedPipe::OUTBOARD_TX,tran[0]->getFifoName(ThreadedPipe::INBOARD_RX));
			else if(tran[1] != NULL)
				block->addFifo(ThreadedPipe::OUTBOARD_TX,tran[1]->getFifoName(ThreadedPipe::INBOARD_RX));
			else if((comm[0] != NULL) && (comm[0]->mode() == ThreadedPipe::DUPLEX)) 
				block->addFifo(ThreadedPipe::OUTBOARD_TX,comm[0]->getFifoName(ThreadedPipe::INBOARD_RX));
			else if(comm[1] != NULL)
				block->addFifo(ThreadedPipe::OUTBOARD_TX,comm[1]->getFifoName(ThreadedPipe::INBOARD_RX));
			//      else 
			//        block->addFifo(ThreadedPipe::OUTBOARD_TX);

			break;
		case ThreadedPipe::DUPLEX:
			block->addFifo(ThreadedPipe::INBOARD_RX);
			if((tran[0] != NULL) && (tran[0]->mode() == ThreadedPipe::DUPLEX)) 
				block->addFifo(ThreadedPipe::OUTBOARD_TX,tran[0]->getFifoName(ThreadedPipe::INBOARD_RX));
			else if(tran[1] != NULL)
				block->addFifo(ThreadedPipe::OUTBOARD_TX,tran[1]->getFifoName(ThreadedPipe::INBOARD_RX));
			else if((comm[0] != NULL) && (comm[0]->mode() == ThreadedPipe::DUPLEX)) 
				block->addFifo(ThreadedPipe::OUTBOARD_TX,comm[0]->getFifoName(ThreadedPipe::INBOARD_RX));
			else if(comm[1] != NULL)
				block->addFifo(ThreadedPipe::OUTBOARD_TX,comm[1]->getFifoName(ThreadedPipe::INBOARD_RX));
			//      else 
			//        block->addFifo(ThreadedPipe::OUTBOARD_TX);

			// Fall through to RX

		case ThreadedPipe::RX:
			block->addFifo(ThreadedPipe::INBOARD_TX);
			if(tran[0] != NULL)
				block->addFifo(ThreadedPipe::OUTBOARD_RX,tran[0]->getFifoName(ThreadedPipe::INBOARD_TX));
			else if(comm[0] != NULL)
				block->addFifo(ThreadedPipe::OUTBOARD_RX,comm[0]->getFifoName(ThreadedPipe::INBOARD_TX));
			//      else 
			//        block->addFifo(ThreadedPipe::OUTBOARD_RX);
			break;
	}
}

String Interface::getFifo(InputType type)
{
	ThreadedPipe::InputType pipe_fifo = ThreadedPipe::OUTBOARD_RX; 
	switch(type) {
		case TX: pipe_fifo = ThreadedPipe::INBOARD_TX; break;
		case RX: pipe_fifo = ThreadedPipe::INBOARD_RX; break;
	}
	if(comp != NULL)
		return comp->getFifoName(pipe_fifo);

	switch(type) {
		case RX:
			if(tran[1] != NULL)
				return tran[1]->getFifoName(pipe_fifo);
			else if((tran[0] != NULL) && (tran[0]->mode() == ThreadedPipe::DUPLEX))
				return tran[0]->getFifoName(pipe_fifo);
		case TX:
			if(tran[0] != NULL)
				return tran[0]->getFifoName(pipe_fifo);
	}

	switch(type) {
		case RX:
			if(comm[1] != NULL)
				return comm[1]->getFifoName(pipe_fifo);
			else if((comm[0] != NULL) && (comm[0]->mode() == ThreadedPipe::DUPLEX))
				return comm[0]->getFifoName(pipe_fifo);
		case TX:
			if(comm[0] != NULL){
				return comm[0]->getFifoName(pipe_fifo);
			}
	}

	return String("");
}

void Interface::registerCapabilities(ShmStruct * shm)
{
	int i = 0;
	SystemCharacter_t * system;
	while((system = my_systems.get(i)) != NULL) {
		shm->capabilities.add(system);
		i++;
	}
}

// Note, for run rates we choose to run the communication blocks
// at a much faster rate so as to minimize any affect of timing
// on control loops waiting for communications.
void Interface::start(ShmStruct * shm)
{

	//cout << "Interface::start" << endl;
	bool status;

	if(comm[0] != NULL){
		status = comm[0]->init(shm);
		if ( status == false) {
			cerr << endl << "\tERROR - " << comm[0]->getName() << " init" << endl;
			comm[0]->shutdown();
		} else if ( comm[0]->start(500, 0) != OK )
			cerr << endl << "\tERROR - " << comm[0]->getName() << " start" << endl;
		usleep(1000);
	}
	if(comm[1] != NULL){
		status = comm[1]->init(shm);
		if ( status == false) {
			cerr << endl << "\tERROR - " << comm[1]->getName() << " init" << endl;
			comm[1]->shutdown();
		} else if ( comm[1]->start(500, 0) != OK )
			cerr << endl << "\tERROR - " << comm[1]->getName() << " start" << endl;
		usleep(1000);
	}

	if(tran[0] != NULL){
		status = tran[0]->init(shm);
		if ( status == false) {
			cerr << endl << "\tERROR - " << tran[0]->getName() << " init" << endl;
			tran[0]->shutdown();
		} else if ( tran[0]->start(500, 0) != OK )
			cerr << endl << "\tERROR - " << tran[0]->getName() << " start" << endl;
		usleep(1000);
	}
	if(tran[1] != NULL){
		status = tran[1]->init(shm);
		if ( status == false) {
			cerr << endl << "\tERROR - " << tran[1]->getName() << " init" << endl;
			tran[1]->shutdown();
		} else if ( tran[1]->start(500, 0) != OK )
			cerr << endl << "\tERROR - " << tran[1]->getName() << " start" << endl;
		usleep(1000);
	}

	if(comp != NULL){
		status = comp->init(shm);
		if ( status == false) {
			cerr << endl << "\tERROR - " << comp->getName() << " init" << endl;
			comp->shutdown();
		} else if ( comp->start(500, 0) != OK )
			cerr << endl << "\tERROR - " << comp->getName() << " start" << endl;
		usleep(1000);
	}
}

void Interface::stop()
{
	//cout << "Interface::stop" << endl;
	if(comm[0] != NULL){
		if(comm[0]->isRunning()) {
			comm[0]->shutdown( );
			if(pthread_join (comm[0]->getTid(), NULL) != 0 )
				perror("pthread_join socket");
			//if(verbose) 
			//comm[0]->displayStats();
		}
	}
	if(comm[1] != NULL){
		if(comm[1]->isRunning()) {
			comm[1]->shutdown( );
			if(pthread_join (comm[1]->getTid(), NULL) != 0 )
				perror("pthread_join socket");
			//if(verbose) 
			//comm[1]->displayStats();
		}
	}
	if(tran[0] != NULL){
		if(tran[0]->isRunning()) {
			tran[0]->shutdown( );
			if(pthread_join (tran[0]->getTid(), NULL) != 0 )
				perror("pthread_join socket");
			//if(verbose) 
			//tran[0]->displayStats();
		}
	}
	if(tran[1] != NULL){
		if(tran[1]->isRunning()) {
			tran[1]->shutdown( );
			if(pthread_join (tran[1]->getTid(), NULL) != 0 )
				perror("pthread_join socket");
			//if(verbose) 
			//tran[1]->displayStats();
		}
	}
	if(comp != NULL){
		if(comp->isRunning()) {
			comp->shutdown( );
			if(pthread_join (comp->getTid(), NULL) != 0 )
				perror("pthread_join socket");
			//if(verbose) 
			//comp->displayStats();
		}
	}
}
